/*
 * @file engine_controller.cpp
 *
 * @brief control the platform.
 * @note based on the DPR2 code.
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date 5 Dec 2012
 */

#include "rp_motor/wheels.h"

// constructor
EngineController::EngineController():
	nh_("~")
{
	init();
}

// destructor
EngineController::~EngineController() {
	delete left_motor_;
	delete right_motor_;
	nh_.shutdown();
}

void EngineController::init() {
	ROS_INFO("Initializing base");

	// Read parameters
	std::string motor_port_name, motor_config_name;

	ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
	ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));
	ROS_ASSERT(nh_.getParam("wheel_diameter", wheel_diameter_));
	ROS_ASSERT(nh_.getParam("wheel_base", wheel_base_));

	ROS_ASSERT(nh_.getParam("pid_kp", Kp_));
	ROS_ASSERT(nh_.getParam("pid_ki", Ki_));
	ROS_ASSERT(nh_.getParam("pid_kd", Kd_));
	ROS_ASSERT(nh_.getParam("pid_ki_limit", Ki_limit_));

	// Subscript to command topic
	vel_sub_ = nh_.subscribe(Topic::NAV_VELOCITY, 10, &EngineController::velocityCallback, this);
	posX_sub_ = nh_.subscribe(Topic::NAV_LINEAR, 10, &EngineController::positionXCallback, this);
	posZ_sub_ = nh_.subscribe(Topic::NAV_ANGULAR, 10, &EngineController::positionZCallback, this);
	vel_pub_ = nh_.advertise<geometry_msgs::Twist>(Topic::WHEELS_STATUS, 10);

	// Load motor configuration
	CXMLConfiguration motor_config_xml;
	ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	CDxlConfig motor_config_left;
	motor_config_left.readConfig(motor_config_xml.root().section("left"));
	left_motor_ = new C3mxlROS(motor_port_name.c_str());
	left_motor_->setConfig(&motor_config_left);	

	// Initialize left motor
	ros::Rate init_rate(1);
	while (ros::ok() && left_motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize left wheel motor, will continue trying every second");
		init_rate.sleep();
	}
	left_motor_->setPIDSpeed(Kp_, Kd_, Ki_, Ki_limit_);

	CDxlConfig motor_config_right;
	motor_config_right.readConfig(motor_config_xml.root().section("right"));
	right_motor_ = new C3mxlROS(motor_port_name.c_str());
	right_motor_->setConfig(&motor_config_right);

	// Initialize right motor
	while (ros::ok() && right_motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize right wheel motor, will continue trying every second");
		init_rate.sleep();
	}
	right_motor_->setPIDSpeed(Kp_, Kd_, Ki_, Ki_limit_);
	motor_mode_ = motor_config_right.m3mxlMode;
}

void EngineController::spin() {
  ROS_INFO("Spinning");
  ros::Rate loop_rate(20);

  while (ros::ok()) {
	  publishMeasuredVelocity();
	  ros::spinOnce();
	  loop_rate.sleep();
  }
}

void EngineController::positionXCallback(const std_msgs::Float32::ConstPtr &msg)
{
	// Calculate wheel velocities
	  double pos_left  = msg->data / 0.15;
	  double pos_right = msg->data / 0.15;

	  setPosition(pos_left, pos_right, 1.0);
}

void EngineController::positionZCallback(const std_msgs::Float32::ConstPtr &msg)
{
  // Calculate wheel velocities
  double pos_left  = msg->data *  (1/0.6);
  double pos_right = msg->data * -(1/0.6);

  setPosition(pos_left, pos_right, 0.5);
}

void EngineController::setPosition(double pos_left, double pos_right, double speed)
{
  if (motor_mode_ != POSITION_MODE)
  {
    DXL_SAFE_CALL(left_motor_->set3MxlMode(POSITION_MODE));
    DXL_SAFE_CALL(right_motor_->set3MxlMode(POSITION_MODE));
    motor_mode_ = POSITION_MODE;
  }

  left_motor_->getPos();
  right_motor_->getPos();

  // Actuate
  left_motor_->setPos(left_motor_->presentPos() + pos_left, speed);
  right_motor_->setPos(right_motor_->presentPos() + pos_right, speed);

  ROS_DEBUG_STREAM("Delta base position set to [" << pos_left << ", " << pos_right << "]");
}

void EngineController::velocityCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
  // Base is nonholonomic, warn if sent a command we can't execute
  if (msg->linear.y || msg->linear.z || msg->angular.x || msg->angular.y)
  {
	ROS_WARN("I'm afraid I can't do that, Dave.");
	return;
  }

  // Calculate wheel velocities
  double vel_linear  = msg->linear.x/(wheel_diameter_/2);
  double vel_angular = msg->angular.z * (wheel_base_/wheel_diameter_);

  double vel_left  = 0;
  double vel_right = 0;

  vel_left    = vel_linear - vel_angular;
  vel_right   = vel_linear + vel_angular;

  if (motor_mode_ != SPEED_MODE)
  {
    DXL_SAFE_CALL(left_motor_->set3MxlMode(SPEED_MODE));
    DXL_SAFE_CALL(right_motor_->set3MxlMode(SPEED_MODE));
    motor_mode_ = SPEED_MODE;
  }

  // Actuate
  left_motor_->setSpeed(vel_left);
  right_motor_->setSpeed(vel_right);

  ROS_DEBUG_STREAM("Base velocity set to [" << vel_left << ", " << vel_right << "]");
}

void EngineController::publishMeasuredVelocity()
{
	geometry_msgs::Twist msg;
	left_motor_->getState();
	right_motor_->getState();

	double vel_left_rad = left_motor_->presentSpeed(); // rad/s
	double vel_right_rad = right_motor_->presentSpeed(); // rad/s

	double vel_left  = vel_left_rad  * (wheel_diameter_/2); // m/s
	double vel_right = vel_right_rad * (wheel_diameter_/2); // m/s
	msg.linear.x = (vel_right + vel_left) / 2; // m/s

	// calculate orientation
	msg.angular.z = (vel_right_rad - vel_left_rad) / (2*wheel_base_/wheel_diameter_); // rad/s

	vel_pub_.publish(msg);
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "engine_node");

	EngineController ec;
	ec.spin();
}
